/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "scene/include/glosa.h"

#include <cmath>
#include <mutex>

#include <glog/logging.h>

#include "google/protobuf/text_format.h"
#include "scene/include/hv_in_map.h"
#include "scene/include/msg_verify.h"

#include "v2xpb-asn-bsm.pb.h"
#include "v2xpb-asn-map.pb.h"
#include "v2xpb-asn-position.pb.h"
#include "v2xpb-asn-spat.pb.h"

namespace v2x {
namespace scene {
bool Glosa::GetGlosaActive() { return this->glosa_active_; }
double Glosa::GetMaximumSpeed() { return this->maximum_speed_; }
double Glosa::GetMinimumSpeed() { return this->minimum_speed_; }
double Glosa::GetRecommendSpeed() { return this->recommend_speed_; }
bool Glosa::IsGlosa(const ::v2xpb::asn::Bsm& bsm,
                    const ::v2xpb::asn::MapLane& lane,
                    const ::v2xpb::asn::SpatIntersection& spat_intersection) {
  std::lock_guard<std::mutex> lock(lock_);
  MsgVerify msg_verify;
  // const auto lane = hv_in_map.GetMapLane();
  const auto& pose = bsm.pos();
  const auto& heading = bsm.heading();
  const auto& speed = bsm.speed();
  const auto& positions = lane.positions();
  const auto& pose_size = lane.positions_size();
  // auto spat_phase = std::make_shared<::v2xpb::asn::SpatPhase>();
  auto spat_phase = msg_verify.GetSpatPhaseId(spat_intersection, lane);
  if (!spat_phase) {
    LOG(ERROR) << "the spats of the lane are not same";
    return false;
  }
  LOG(INFO) << "pose_size is " << pose_size;
  if (pose_size < 2) {
    LOG(ERROR) << "number of lane's position is less than 2, return false";
    return false;
  }
  double distance = CalculateDistance(pose, heading, speed, lane);
  std::vector<::v2xpb::asn::SpatPhaseState> spat_phase_vec;
  for (int i = 0; i < spat_phase->phase_state_size(); ++i) {
    spat_phase_vec.push_back(spat_phase->phase_state(i));
  }
  double speed_limitation = 100;
  if (lane.speed_limits_size() != 0) {
    if (lane.speed_limits(0).has_speed()) {
      speed_limitation = lane.speed_limits(0).speed();
    }
    for (int i = 1; i < lane.speed_limits_size(); ++i) {
      if (lane.speed_limits(i).has_speed()) {
        if (lane.speed_limits(i).speed() < speed_limitation) {
          speed_limitation = lane.speed_limits(i).speed();
        }
      }
    }
  }
  LOG(INFO) << "the speed_limitation is " << speed_limitation;
  CalculateSpeedRange(spat_phase_vec, speed, distance, speed_limitation);
  CalculateRecommendSpeed(speed_limitation);
  return true;
}
double Glosa::CalculateDistance(const v2xpb::asn::Position& pose,
                                double heading, double speed,
                                const ::v2xpb::asn::MapLane& lane) {
  int pose_size = lane.positions_size();
  double last_second_x = lane.positions(pose_size - 2).xyz().x();
  double last_second_y = lane.positions(pose_size - 2).xyz().y();
  double last_x = lane.positions(pose_size - 1).xyz().x();
  double last_y = lane.positions(pose_size - 1).xyz().y();
  const double pi = M_PI;
  double lane_stop_line_angle =
      std::atan2(last_y - last_second_y, last_x - last_second_x);
  double run_angle = std::fabs(lane_stop_line_angle - heading);
  if (run_angle > M_PI) {
    run_angle = 2 * M_PI - run_angle;
  }
  double delta_x = last_x - pose.xyz().x();
  double delta_y = last_y - pose.xyz().y();
  double linear_distance = std::sqrt(delta_x * delta_x + delta_y * delta_y);
  LOG(INFO) << "linear_distance is " << linear_distance;
  LOG(INFO) << "run_angle is " << run_angle;
  double distance = linear_distance / (2 * std::sin(run_angle * 0.5));
  LOG(INFO) << "the distance between hv and stop line is " << distance;
  return distance;
}
void Glosa::CalculateSpeedRange(
    const std::vector<::v2xpb::asn::SpatPhaseState>& spat_phase_vec,
    double speed, double distance, double speed_limitation) {
  double speed_no_stop = 0;
  double next_green_time = 0;
  switch (spat_phase_vec[0].color()) {
    case ::v2xpb::asn::SpatPhaseColor::COLOR_PERMISSIVE_GREEN:
    case ::v2xpb::asn::SpatPhaseColor::COLOR_PROTECTED_GREEN:
      if (spat_phase_vec[0].timing_end() == 0 && distance == 0) {
        maximum_speed_ = 0;
        minimum_speed_ = 0;
      } else {
        double rush_pass_time = distance / speed_limitation;
        if (spat_phase_vec[0].timing_end() > rush_pass_time) {
          maximum_speed_ = speed_limitation;
          minimum_speed_ = distance / spat_phase_vec[0].timing_end();
        } else {
          minimum_speed_ = 0;
          // time remained before next green spat end
          next_green_time = spat_phase_vec[0].timing_end();
          for (auto& spat : spat_phase_vec) {
            next_green_time += spat.timing_duration();
          }
          // speed the car can pass the cross with no stop
          speed_no_stop = distance / next_green_time;
          maximum_speed_ = speed_no_stop < speed_limitation ? speed_no_stop
                                                            : speed_limitation;
        }
      }
      LOG(INFO) << "the speed to the lane without stop is " << speed_no_stop;
      break;
    case ::v2xpb::asn::SpatPhaseColor::COLOR_YELLOW:
    case ::v2xpb::asn::SpatPhaseColor::COLOR_FLASHING_YELLOW:
      minimum_speed_ = 0;
      next_green_time = spat_phase_vec[0].timing_end();
      for (int i = 1; i < spat_phase_vec.size(); ++i) {
        next_green_time += spat_phase_vec[i].timing_duration();
        if (spat_phase_vec[i].color() ==
                ::v2xpb::asn::SpatPhaseColor::COLOR_PERMISSIVE_GREEN ||
            spat_phase_vec[i].color() ==
                ::v2xpb::asn::SpatPhaseColor::COLOR_PROTECTED_GREEN) {
          speed_no_stop = distance / next_green_time;
          maximum_speed_ = speed_no_stop < speed_limitation ? speed_no_stop
                                                            : speed_limitation;
          break;
        }
      }
      LOG(WARNING) << "the speed to the lane without stop is " << speed_no_stop;
      break;
    case ::v2xpb::asn::SpatPhaseColor::COLOR_RED:
      minimum_speed_ = 0;
      next_green_time = spat_phase_vec[0].timing_end();
      for (int i = 1; i < spat_phase_vec.size(); ++i) {
        next_green_time += spat_phase_vec[i].timing_duration();
        if (spat_phase_vec[i].color() ==
                ::v2xpb::asn::SpatPhaseColor::COLOR_PERMISSIVE_GREEN ||
            spat_phase_vec[i].color() ==
                ::v2xpb::asn::SpatPhaseColor::COLOR_PROTECTED_GREEN) {
          speed_no_stop = distance / next_green_time;
          maximum_speed_ = speed_no_stop < speed_limitation ? speed_no_stop
                                                            : speed_limitation;
          break;
        }
      }
      LOG(INFO) << "next_green_time is " << next_green_time;
      LOG(INFO) << "the speed to the lane without stop is " << speed_no_stop;
      break;
    default:
      LOG(INFO) << "invalid spat color";
      maximum_speed_ = speed_limitation;
      minimum_speed_ = 0;
      break;
  }
  LOG(INFO) << "max speed is " << maximum_speed_ << ", min speed is "
            << minimum_speed_;
}

void Glosa::CalculateRecommendSpeed(double speed_limitation) {
  LOG(INFO) << "the speed limitation is " << speed_limitation;
  double estimated_speed = (maximum_speed_ + minimum_speed_) * 0.5;
  double remainder = std::fmod(estimated_speed, 5.0);
  if (remainder > 2.5) {
    // ceil division
    estimated_speed += (5 - remainder);
  } else {
    // floor division
    estimated_speed -= remainder;
  }
  if (estimated_speed < minimum_speed_ || estimated_speed > maximum_speed_) {
    remainder = std::fmod(maximum_speed_, 5.0);
    estimated_speed = maximum_speed_ + (5 - remainder);
  }
  recommend_speed_ = std::round(estimated_speed) < speed_limitation
                         ? std::round(estimated_speed)
                         : speed_limitation;
  if (recommend_speed_ <= 0) {
    recommend_speed_ = 5;
  }
  LOG(INFO) << "recommend speed is " << recommend_speed_;
}
}  // namespace scene
}  // namespace v2x
